/*
实现发布两只乌龟的坐标信息（TransformStamped,区别于点PointStamped）
四步骤：
1.订阅乌龟的位置主题，
2.创建乌龟坐标系数据
3.发布乌龟的坐标系数据
4.封装一下，可以发多只乌龟
*/

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"

std::string turtle_name;

void doPoseCallback(const turtlesim::Pose::ConstPtr &pose)
{
    static tf2_ros::TransformBroadcaster caster;
    //组织发布数据
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";
    tfs.child_frame_id = turtle_name;
    tfs.header.stamp = ros::Time::now();
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0;
    tf2::Quaternion qtn;
    qtn.setRPY(0, 0, pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();

    //发送数据
    caster.sendTransform(tfs);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "pub_turtle_coordinate_frame");
    if (argc != 2)
    {
        ROS_ERROR("请传入正确参数!");
        return -1;
    }
    turtle_name = argv[1]; //动态获取乌龟的名字
    ros::NodeHandle nh;
    //订阅乌龟位姿主题
    //这个变量 sub_pos 必须有，如果没有就动态被回收了
    ros::Subscriber sub_pos = nh.subscribe(turtle_name + "/pose", 10, doPoseCallback);
    ros::spin();
    return 0;
}
